#include "MultibodySystem.h"

bool MBSystem::Initialize()
{
	peq->Initialize();
	L.resize(nb);
	for (unsigned int i = 0; i < nb; ++i)
	{
		L[i] = jointvec[i]->Bj->id - 1;
	}
	AUX::Inscribed2Associative(L, S);
	AUX::Inscribed2Path(L, T);
	alpha.resize(nb);
	beta.resize(nb);
	for (unsigned int i = 0; i < nb; ++i)
	{
		alpha[i].resize(nb);
		beta[i].resize(nb);
		for (unsigned int j = 0; j < nb; ++j)
		{
			alpha[i][j].setZero(3, jointvec[j]->DOF());
			beta[i][j].setZero(3, jointvec[j]->DOF());
		}
	}
	massvec.resize(nb, 0);
	CalMass();
	Jcvec.resize(nb, Matrix3d::Zero());
	Fo.resize(nb, Vector3d::Zero());
	Mo.resize(nb, Vector3d::Zero());
	Ft.resize(dof);
	Ft.setZero();
	w.resize(nb, Vector3d::Zero());
	sigma.resize(nb, Vector3d::Zero());
	epsilon.resize(nb, Vector3d::Zero());
	d.resize(nb, vector<Vector3d>(nb, Vector3d::Zero()));
	Cald();
	dstar.resize(nb, vector<Vector3d>(nb, Vector3d::Zero()));
	Ho.resize(nb);
	Hh.resize(nb);
	for (unsigned int i = 0; i < nb; ++i)
	{
		Ho[i].setZero(3, jointvec[i]->DOF());
		Hh[i].setZero(3, jointvec[i]->DOF());
	}
	Z.resize(dof, dof);
	z.resize(dof);
	Z.setZero();
	z.setZero();
	ypre.resize(dof);
	dypre.resize(dof);
	ypre.setConstant(100);
	dypre.setConstant(100);
	double* yi = ypre.data();
	double* dyi = dypre.data();
	ydim.resize(nb);
	ycum.resize(nb);
	for (unsigned int i = 0; i < nb; ++i)
	{
		ydim[i] = jointvec[i]->DOF();
		jointvec[i]->yi = yi;
		jointvec[i]->dyi = dyi;
		if (i != 0)
			ycum[i] = ycum[i - 1] + jointvec[i-1]->DOF();
		else
			ycum[0] = 0;
		yi += ydim[i];
		dyi += ydim[i];
	}
	q.resize(nb * Body::NC);
	dq.resize(nb * Body::NC);
	for (unsigned int i = 0; i < nb; ++i)
	{
		jointvec[i]->Bi->pos = q.data() + i * Body::NC;
		jointvec[i]->Bi->vel = dq.data() + i * Body::NC;
	}
	return true;
}

bool MBSystem::Update(IN double t, IN double* y, IN double* dy)
{
	bool isyeq = AUX::isEqual(y, ypre.data(), dof);
	bool isdyeq = AUX::isEqual(dy, dypre.data(), dof);
	if (isyeq && isdyeq)
		return false;
	if (!isyeq)
	{
		for (int i = 0; i < ypre.rows(); ++i)
			ypre(i) = y[i];
		Calytoq(t, y);
	}
	if (!isdyeq)
	{
		for (int i = 0; i < dypre.rows(); ++i)
			dypre(i) = dy[i];
		Caldytodq(t, dy);
	}
	CalHo();
	CalHh();
	CalJc();
	Caldstar();
	Calepsilon();
	Calsigma(t);
	CalMo(t);
	CalFo(t);
	CalFt(t);
	Calw(t);
	Calbeta();
	Calalpha();
	return true;
}

bool MBSystem::Calytoq(IN double t, IN double* y)
{
	for (unsigned int i = 0; i < nb; ++i)
		jointvec[i]->calytoq(t, y + ycum[i]);
	return true;
}

bool MBSystem::Caldytodq(IN double t, IN double* dy)
{
	for (unsigned int i = 0; i < nb; ++i)
		jointvec[i]->caldytodq(t, dy + ycum[i]);
	return true;
}


MBSystem::MBSystem()
{
	dof = 0;
	nb = 0;
	nc = 0;
	peq = new Equation(this);
	psolver = new Solver(peq);
}

MBSystem::~MBSystem()
{
	if (peq)
		delete peq;
	if (psolver)
		delete psolver;
}

MatrixXd& MBSystem::CalZ(IN double t, IN double* y, IN double* dy)
{
	//update before calculate Z.
	Update(t, y, dy);
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			Z.block(ycum[i], ycum[j], ydim[i], ydim[j]).setZero();
			for (unsigned int k = 0; k < nb; ++k)
			{
				Z.block(ycum[i], ycum[j], ydim[i], ydim[j]) +=
					alpha[k][i].transpose() * massvec[k] * alpha[k][j];
				Z.block(ycum[i], ycum[j], ydim[i], ydim[j]) +=
					beta[k][i].transpose() * Jcvec[k] * beta[k][j];
			}
		}
	}
	//cout << Z << endl;
	return Z;
}

VectorXd& MBSystem::Calz(IN double t, IN double* y, IN double* dy)
{
	//update before calculate z.
	Update(t, y, dy);
	z.setZero();
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			z.segment(ycum[i], ydim[i]) +=
				alpha[j][i].transpose() * (Fo[j] - massvec[j] * w[j]);
			z.segment(ycum[i], ydim[i]) +=
				beta[j][i].transpose() * (Mo[j] - Jcvec[j] * sigma[j] - epsilon[j]);
		}
	}
	//cout << z(1) << " ";
	z += Ft;
	//cout << z(1) << endl;
	return z;
}

bool MBSystem::Add(IN JointBase* j)
{
	if (std::find(jointvec.begin(), jointvec.end(), j) != jointvec.end())
		return false;
	dof += j->DOF();
	nb += 1;
	nc += Body::NC;
	if (jointvec.empty())
	{
		jointvec.push_back(j);
		return true;
	}
	auto it = jointvec.begin();
	for (; it != jointvec.end(); ++it)
	{
		if ((*it)->Bi->id > j->Bi->id)
		{
			jointvec.insert(it, j);
			return true;
		}
	}
	jointvec.insert(it, j);
	return true;
}

bool MBSystem::Del(IN JointBase* j)
{
	auto it = std::find(jointvec.begin(), jointvec.end(), j);
	if (it == jointvec.end())
		return false;
	dof -= (*it)->DOF();
	nb -= 1;
	nc -= Body::NC;
	jointvec.erase(it);
	return true;
}

bool MBSystem::Add(IN ForceBase* f)
{
	auto it = std::find(forcevec.begin(), forcevec.end(), f);
	if (it != forcevec.end())
		return false;
	forcevec.push_back(f);
	return true;
}

bool MBSystem::Del(IN ForceBase* f)
{
	auto it = std::find(forcevec.begin(), forcevec.end(), f);
	if (it == forcevec.end())
		return false;
	forcevec.erase(it);
	return true;
}

bool MBSystem::Add(IN JointForce* jf)
{
	auto it = std::find(jforcevec.begin(), jforcevec.end(), jf);
	if (it != jforcevec.end())
		return false;
	jforcevec.push_back(jf);
	return true;
}

bool MBSystem::Del(IN JointForce* jf)
{
	auto it = std::find(jforcevec.begin(), jforcevec.end(), jf);
	if (it == jforcevec.end())
		return false;
	jforcevec.erase(it);
	return true;
}

bool MBSystem::Sety0(IN VectorXd& _y0)
{
	y0 = _y0;
	return true;
}

bool MBSystem::Setdy0(IN VectorXd& _dy0)
{
	dy0 = _dy0;
	return true;
}

bool MBSystem::SetTimeInterval(IN double ti, IN double te, IN int N)
{
	if (N < 0)
		return false;
	return psolver->SetTime(ti, te, N);
}

bool MBSystem::setTolerance(IN double r, IN double a)
{
	return psolver->SetTolerance(a, r);
}

bool MBSystem::SaveAs(IN string fname, IN bool isbinary)
{
	ofstream fout;
	if (isbinary)
	{
		fname.append(".dat");
		fout.open(fname, ios::binary | ios::out);
	}
	else
	{
		fname.append(".txt");
		fout.open(fname, ios::out);
	}
	if (!fout)
	{
		cerr << "Open output file error!" << endl;
		return false;
	}
	unsigned int NT = psolver->tspan.size();

	if (isbinary)
	{
		fout.write(reinterpret_cast<char*>(&NT), sizeof(unsigned int));
		fout.write(reinterpret_cast<char*>(&dof), sizeof(unsigned int));
		fout.write(reinterpret_cast<char*>(&nc), sizeof(unsigned int));
		for (auto& item : psolver->tspan)
			fout.write(reinterpret_cast<char*>(&item), sizeof(double));
		for (auto& item : psolver->Y)
			fout.write(reinterpret_cast<char*>(&item), dof * sizeof(double));
		for (auto& item : psolver->DY)
			fout.write(reinterpret_cast<char*>(&item), dof * sizeof(double));
		for (auto& item : psolver->DY)
			fout.write(reinterpret_cast<char*>(item.data() + dof), dof * sizeof(double));
		for (auto& item : psolver->Q)
			fout.write(reinterpret_cast<char*>(item.data()), nc * sizeof(double));
		for (auto& item : psolver->DQ)
			fout.write(reinterpret_cast<char*>(item.data()), nc * sizeof(double));
	}
	else
	{
		fout << NT << endl;
		fout << dof << endl;
		fout << nc << endl;
		for (auto& item : psolver->tspan)
			fout << item << endl;
		for (auto& item : psolver->Y)
			fout << item.head(dof).transpose() << endl;
		for (auto& item : psolver->DY)
			fout << item.head(dof).transpose() << endl;
		for (auto& item : psolver->DY)
			fout << item.tail(dof).transpose() << endl;
		for (auto& item : psolver->Q)
			fout << item.transpose() << endl;
		for (auto& item : psolver->DQ)
			fout << item.transpose() << endl;
	}
	fout.close();
	return true;
}

unsigned int MBSystem::DOF() const
{
	return dof;
}

bool MBSystem::Calculate()
{
	if (!Initialize())
		return false;
	if (!psolver->Calculate())
		return false;
	return true;
}

bool MBSystem::SetToInitial()
{
	Update(psolver->t_ini, y0.data(), dy0.data());
	return true;
}
/*check once*/
bool MBSystem::CalMass()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		massvec[i] = jointvec[i]->Bi->TotalMass();
	}
	return true;
}
/*check once*/
bool MBSystem::Cald()
{
	/*cal C first*/
	vector<vector<Vector3d>> C(nb, vector<Vector3d>(nb, Vector3d::Zero()));
	for (unsigned int i = 0; i < nb; ++i)
	{
		C[i][i] = -jointvec[i]->rhoi;
		if (jointvec[i]->Bj->Type() != Body::BASE)
		{
			int I = jointvec[i]->Bi->id - 1;
			int J = jointvec[i]->Bj->id - 1;
			C[J][I] = jointvec[i]->rhoj;
		}
	}
	/*cal d*/
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			d[i][j].setZero();
			for (unsigned int k = 0; k < nb; ++k)
				d[i][j] += -C[i][k] * T(k,j);
		}
	}
	/*for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
			cout << d[i][j].transpose() << " ";
		cout << endl;
	}*/
	return true;
}
/*check once*/
bool MBSystem::CalHo()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		jointvec[i]->CalHoT(Ho[i]);
	}
	return true;
}
/*check once*/
bool MBSystem::CalHh()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		jointvec[i]->CalHhT(Hh[i]);
	}
	return true;
}
/*check once*/
bool MBSystem::CalJc()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		Jcvec[i] = static_cast<RigidBody*>(jointvec[i]->Bi)->CalJc();
	}
	return true;
}
/*check once*/
bool MBSystem::Caldstar()
{
	Matrix3d Ak, Ah0;
	for (unsigned int k = 0; k < nb; ++k)
	{
		jointvec[k]->Bi->A(Ak);
		for (unsigned int i = k; i < nb; ++i)
		{
			if (i == k)
			{
				dstar[k][k] = Ak * d[k][k];
			}
			else
			{
				for (unsigned int l = k + 1; l <= i; ++l)
				{
					if (T(l, i) != 0&&T(k,l)!=0)
					{
						jointvec[l]->calAh0(Ah0);
						dstar[k][i] = Ak * d[k][i] + Ah0 * jointvec[l]->Hi();
						break;
					}
				}
			}
		}
	}
	return true;
}
/*check once*/
bool MBSystem::Calepsilon()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		static_cast<RigidBody*>(jointvec[i]->Bi)->Calepsilon(epsilon[i]);
	}
	/*for (auto& item : epsilon)
		cout << item.transpose() << endl;*/
	return true;
}
/*check once*/
bool MBSystem::Calsigma(IN double t)
{
	vector<Vector3d> yz(nb);
	for (unsigned int i = 0; i < nb; ++i)
		yz[i] = jointvec[i]->YitaPlusZeta();
	Vector3d w0dot = static_cast<BaseBody*>(jointvec[0]->Bj)->Acceleration(t).tail<3>();
	for (unsigned int i = 0; i < nb; ++i)
	{
		sigma[i] = w0dot;
		for (unsigned int j = 0; j < nb; ++j)
		{
			if (T(j, i) == 0)
				continue;
			sigma[i] -= T(j, i) * yz[j];
		}
	}
	return true;
}
/*check once*/
bool MBSystem::CalMo(IN double t)
{
	if (forcevec.empty())
		return true;
	for (unsigned int i = 0; i < nb; ++i)
		Mo[i].setZero();
	for (auto& pf : forcevec)
	{
		int i = pf->Bi_Id() - 1;
		Mo[i] += pf->CalTorque(t, nullptr, nullptr);
	}
	return true;
}
/*check once*/
bool MBSystem::CalFo(IN double t)
{
	if (forcevec.empty())
		return true;
	for (unsigned int i = 0; i < nb; ++i)
		Fo[i].setZero();
	for (auto& pf : forcevec)
	{
		int i = pf->Bi_Id() - 1;
		Fo[i] += pf->CalForce(t, nullptr, nullptr);
	}
	return true;
}
/*check once*/
bool MBSystem::CalFt(IN double t)
{
	if (jforcevec.empty())
		return true;
	Ft.setZero();
	for (auto& pj : jforcevec)
	{
		int i = pj->Hj->Bi->id - 1;
		Ft.segment(ycum[i], pj->DOF()) += pj->CalForce(t);
	}
	return true;
}

/*check once*/
bool MBSystem::Calw(IN double t)
{
	/*calculate u first*/
	vector<Vector3d> omg(nb);
	for (unsigned int i = 0; i < nb; ++i)
		omg[i] = jointvec[i]->Bi->AngularVel();
	Vector3d w0, w0dot, r0ddot;
	w0 = static_cast<BaseBody*>(jointvec[0]->Bj)->Velocity(t).tail<3>();
	r0ddot = static_cast<BaseBody*>(jointvec[0]->Bj)->Acceleration(t).head<3>();
	w0dot = static_cast<BaseBody*>(jointvec[0]->Bj)->Acceleration(t).tail<3>();
	Vector3d ubase = r0ddot;
	Matrix3d Ah0;
	jointvec[0]->calAh0(Ah0);
	Vector3d h1 = Ah0 * jointvec[0]->Hi();
	ubase += w0dot.cross(h1);
	ubase += w0.cross(w0.cross(h1));
	for (unsigned int i = 0; i < nb; ++i)
	{
		w[i] = ubase;
		for (unsigned int k = 0; k <= i; ++k)
		{
			if (T(k, i) == 0)
				continue;
			if (k == 0)
			{
				jointvec[0]->calAh0(Ah0);
				w[i] += 2 * w0.cross(Ah0*jointvec[0]->Vri());
			}
			else
			{
				jointvec[k]->calAh0(Ah0);
				w[i] += 2 * omg[L[k]].cross(Ah0 * jointvec[k]->Vri());
			}
			w[i] += omg[k].cross(omg[k].cross(dstar[k][i]));
		}
	}
	/*now calculate w*/
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			w[i] -= dstar[j][i].cross(sigma[j]);
		}
	}
	/*for (unsigned int i = 0; i < nb; ++i)
		cout << w[i].transpose() << endl;*/
	return true;
}
/*check once*/
bool MBSystem::Calbeta()
{
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			if (T(j, i) == 0)
				continue;
			beta[i][j] = Ho[j];
		}
	}
	return true;
}
/*check once*/
bool MBSystem::Calalpha()
{
	//ensure beta and dstar is already calculated.
	Matrix3d til;
	for (unsigned int i = 0; i < nb; ++i)
	{
		for (unsigned int j = 0; j < nb; ++j)
		{
			alpha[i][j] = -1 * T(j, i) * Hh[j];
			for (unsigned int k = 0; k < nb; ++k)
			{
				AUX::tilde(dstar[k][i], til);
				alpha[i][j] -= til * beta[k][j];
			}
		}
	}
	return true;
}

Equation::Equation(IN MBSystem* p)
{
	pmbs = p;
	dof = 0;
}

Equation::~Equation()
{
}

bool Equation::Initialize()
{
	dof = 2*pmbs->DOF();
	R.resize(dof);
	L.resize(dof, dof);
	R.setZero();
	L.setZero();
	L.topLeftCorner(dof / 2, dof / 2).setIdentity();
	return true;
}

MatrixXd& Equation::Left(IN double t, IN VectorXd& y)
{
	// update first
	auto& n = pmbs->dof;
	L.block(n, n, n, n) = pmbs->CalZ(t, y.data(), y.data() + n);
	return L;
}

VectorXd& Equation::Right(IN double t, IN VectorXd& y)
{
	// update first
	auto& n = pmbs->dof;
	R.segment(0, n) = y.segment(n, n);
	R.segment(n, n) = pmbs->Calz(t, y.data(), y.data() + n);
	return R;
}

VectorXd Equation::InitialValue() const
{
	VectorXd yini(2 * pmbs->DOF());
	yini << pmbs->y0, pmbs->dy0;
	return yini;
}

unsigned int Equation::DOF() const
{
	return dof;
}

Solver::Solver(IN Equation* p)
{
	peq = p;
	t_ini = 0;
	t_end = 1.0;
	Nstep = 100;
	Atol = 1e-4;
	Rtol = 1e-3;
}

Solver::~Solver()
{
}

bool Solver::SetTime(IN double ti,IN double te,IN unsigned int N)
{
	t_ini = ti;
	t_end = te;
	Nstep = N;
	return true;
}

bool Solver::SetTolerance(IN double atol,IN double rtol)
{
	Atol = atol;
	Rtol = rtol;
	return true;
}

bool Solver::Calculate()
{
	double dt = (t_end - t_ini) / Nstep;
	VectorXd y0 = peq->InitialValue();
	VectorXd y = y0, dy;
	dy.resize(peq->DOF());
	dy.head(peq->DOF() / 2) = y.tail(peq->DOF() / 2);
	dy.tail(peq->DOF() / 2).setZero();
	double t = t_ini;
	unsigned int cnt = 0;
	while (t < t_end)
	{
		tspan.push_back(t);
		Y.push_back(y);
		DY.push_back(dy);
		if (cnt % 1000 == 0)
			cout << "time passed by " << t << " s." << endl;
		dy = peq->Left(t, y).partialPivLu().solve(peq->Right(t, y));
		Q.push_back(peq->pmbs->q);
		DQ.push_back(peq->pmbs->dq);
		y += dy * dt;
		t += dt;
		cnt++;
	}
	return true;
}

MBFileParser::~MBFileParser()
{
	if (pmbs != nullptr && freememo)
	{
		for (auto& item : pmbs->jointvec)
			delete item;
		for (auto& item : pmbs->forcevec)
			delete item;
		for (auto& item : pmbs->jforcevec)
			delete item;
		for (auto& item : bodyvec)
			delete item;
	}
}

bool MBFileParser::Read(IN const string& fname)
{
	ifstream fin(fname, ios::binary);
	if (!fin)
		throw MBException("Open File failed!");
	clear();
	pmbs = new MBSystem();
	Json::Reader reader;
	Json::Value root;
	if (!reader.parse(fin, root))
		throw MBException("Parse json file error!");
	//set Rcords type
	if (root.isMember("RCord"))
	{
		string rtype = root["RCord"].asString();
		if (rtype == "Euler Quaternion")
			Body::SetRcords(RCORDS::EULERQUATERNION);
		else if (rtype == "Euler Angle")
			Body::SetRcords(RCORDS::EULERANGLE);
		else if (rtype == "Cardan Angle")
			Body::SetRcords(RCORDS::CARDANANGLE);
	}
	// create body
	unsigned int nb = root["Body"].size();
	bodyvec.resize(nb, nullptr);
	int id;
	double mass;
	for (unsigned int i = 0; i < nb; ++i)
	{
		Json::Value& body = root["Body"][i];
		string btype = body["Type"].asString();
		id = body["Id"].asInt();
		CheckId(id);
		if (btype == "Base")
		{
			bodyvec[0] = new BaseBody();
			bodyvec[0]->SetID(0);
		}
		else if (btype == "Rigid")
		{
			mass = body["Mass"].asDouble();
			CheckMass(mass);
			Json::Value& Jc = body["Jc"];
			CheckJc(Jc);
			Matrix3d Ic;
			GetJc(Jc, Ic);
			bodyvec[id] = new RigidBody(mass, Ic);
			bodyvec[id]->SetID(id);
		}
		else
		{
			throw MBException("No such type body:" + btype);
		}
	}
	//create Joint
	unsigned int nj = root["Joint"].size();
	vector<VectorXd> posvec;
	vector<VectorXd> velvec;
	posvec.resize(nj);
	velvec.resize(nj);
	for (unsigned int i = 0; i < nj; ++i)
	{
		Json::Value& joint = root["Joint"][i];
		string jtype = joint["Type"].asString();
		int Bi_id = joint["Bi_Id"].asInt();
		int Bj_id = joint["Bj_Id"].asInt();
		Vector3d rhoi, rhoj;
		CheckId(Bi_id);
		CheckId(Bj_id);
		CheckRho(joint["Rhoi"]);
		CheckRho(joint["Rhoj"]);
		GetRho(joint["Rhoi"], rhoi);
		GetRho(joint["Rhoj"], rhoj);
		JointBase* pj = nullptr;
		if (jtype == "Revolute")
		{
			pj = new Revolute(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else if (jtype == "Universe")
		{
			pj = new Universe(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else if (jtype == "Sphere")
		{
			pj = new Sphere(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else if (jtype == "Prism")
		{
			pj = new Prism(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else if (jtype == "Cylinder")
		{
			pj = new Cylinder(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else if (jtype == "Virtual")
		{
			pj = new Virtual(bodyvec[Bi_id], bodyvec[Bj_id], rhoi, rhoj);
		}
		else
		{
			throw MBException("There is no such type joint:" + jtype);
		}
		/*initial position and velocity*/
		CheckPos(joint["Position"], pj->DOF());
		CheckVel(joint["Velocity"], pj->DOF());
		GetPos(joint["Position"], posvec[Bi_id - 1]);
		GetVel(joint["Velocity"], velvec[Bi_id - 1]);
		/*set CiP and CjQ*/
		CheckMat3d(joint["CiP"]);
		CheckMat3d(joint["CjQ"]);
		Matrix3d M;
		GetMat3d(joint["CiP"], M);
		pj->setCiP(M);
		GetMat3d(joint["CjQ"], M);
		pj->setCjQ(M);
		pmbs->Add(pj);
	}
	VectorXd y0, dy0;
	unsigned int dof = 0;
	for (auto& item : posvec)
		dof += item.rows();
	y0.resize(dof);
	dy0.resize(dof);
	unsigned int s = 0;
	for (unsigned int i = 0; i < posvec.size(); ++i)
	{
		y0.segment(s, posvec[i].rows()) = posvec[i];
		dy0.segment(s, velvec[i].rows()) = velvec[i];
		s += posvec[i].rows();
	}
	pmbs->Sety0(y0);
	pmbs->Setdy0(dy0);
	//Create Force
	if (root.isMember("Force"))
	{
		unsigned int nf = root["Force"].size();
		for (unsigned int i = 0; i < nf; ++i)
		{
			Json::Value& force = root["Force"][i];
			string ftype = force["Type"].asString();
			if (ftype == "Gravity")
			{
				int Bi_id = force["Bi_Id"].asInt();
				pmbs->Add(new Gravity(bodyvec[Bi_id]));
			}
			else if (ftype == "JointForce")
			{
				int Bi_id = force["Bi_Id"].asInt();
				CheckForce(force["Magnitude"], pmbs->jointvec[Bi_id - 1]->DOF());
				VectorXd f;
				GetForce(force["Magnitude"], f);
				JointForce* jf = new JointForce(pmbs->jointvec[Bi_id - 1], f);
				pmbs->Add(jf);
			}
			else if (ftype == "ActiveForce")
			{
				int Bi_id = force["Bi_Id"].asInt();
				CheckRho(force["Rho"]);
				CheckForce(force["force"], 3);
				Vector3d rho;
				VectorXd f;
				bool ad = false;
				if (force.isMember("Adhesion"))
					ad = force["Adhesion"].asBool();
				GetRho(force["Rho"], rho);
				GetForce(force["force"], f);
				Vector3d temp = f;
				ForceBase* pf = new ActiveForce(bodyvec[Bi_id], rho, temp, ad);
				pmbs->Add(pf);
			}
			else if (ftype == "ActiveTorque")
			{
				int Bi_id = force["Bi_Id"].asInt();
				CheckForce(force["torque"], 3);
				VectorXd t;
				bool ad = false;
				if (force.isMember("Adhesion"))
					ad = force["Adhesion"].asBool();
				GetForce(force["force"], t);
				Vector3d temp = t;
				ForceBase* pf = new ActiveTorque(bodyvec[Bi_id], temp, ad);
				pmbs->Add(pf);
			}
			else
			{
				throw MBException("No such type force: "+ftype);
			}
		}
	}
	//set simulation time
	Json::Value& simset = root["Simulation"];
	Json::Value& Tspan = simset["Tspan"];
	double tb = Tspan["Start"].asDouble();
	double te = Tspan["End"].asDouble();
	int N = Tspan["Nstep"].asInt();
	if (N <= 0)
		throw MBException("Simulation step number cannot be non-positive!");
	else
		pmbs->SetTimeInterval(tb, te, N);
	if (simset.isMember("Tolerance"))
	{
		if (simset["Tolerance"].isMember("Absolute") && simset["Tolerance"].isMember("Relative"))
			pmbs->setTolerance(simset["Tolerance"]["Relative"].asDouble(), simset["Tolerance"]["Absolute"].asDouble());
	}
	freememo = true;
	return true;
}

bool MBFileParser::Write(IN const string& fname)
{
	if (!pmbs)
		return false;
	pmbs->SetToInitial();
	Json::Value root;
	//write RCord
	if (Body::m_s_rtype == RCORDS::CARDANANGLE)
		root["RCord"] = Json::Value("Cardan Angle");
	else if (Body::m_s_rtype == RCORDS::EULERANGLE)
		root["RCord"] = Json::Value("Euler Angle");
	else if (Body::m_s_rtype == RCORDS::EULERQUATERNION)
		root["RCord"] = Json::Value("Euler Quaternion");
	//write bodies
	auto& jvec = pmbs->jointvec;
	for (unsigned int i = 0; i < jvec.size(); ++i)
	{
		Json::Value body;
		jvec[i]->Bi->Write(body);
		root["Body"].append(body);
		if (jvec[i]->Bj->Type() == Body::BASE)
		{
			Json::Value bd;
			jvec[i]->Bj->Write(bd);
			root["Body"].append(bd);
		}
	}
	//write joints
	for (unsigned int i = 0; i < jvec.size(); ++i)
	{
		Json::Value joint;
		jvec[i]->Write(joint);
		root["Joint"].append(joint);
	}
	//write force
	for (auto& item : pmbs->forcevec)
	{
		Json::Value force;
		item->Write(force);
		root["Force"].append(force);
	}
	for (auto& item : pmbs->jforcevec)
	{
		Json::Value force;
		item->Write(force);
		root["Force"].append(force);
	}
	//simulation setting
	Json::Value Tspan;
	Tspan["Start"] = Json::Value(pmbs->psolver->t_ini);
	Tspan["End"] = Json::Value(pmbs->psolver->t_end);
	Tspan["Nstep"] = Json::Value(pmbs->psolver->Nstep);
	root["Simulation"]["Tspan"] = Tspan;
	Json::Value Tol;
	Tol["Absolute"] = Json::Value(pmbs->psolver->Atol);
	Tol["Relative"] = Json::Value(pmbs->psolver->Rtol);
	root["Simulation"]["Tolerance"] = Tol;
	//write to file
	Json::StyledWriter sw;
	ofstream fout;
	fout.open(fname, ios::out);
	if (!fout)
		throw MBException("Open output json file error!");
	fout << sw.write(root);
	fout.close();
	return true;
}

bool MBFileParser::Simulate()
{
	if (pmbs)
		return pmbs->Calculate();
	else
		return false;
}

bool MBFileParser::SaveDataAs(IN const string& fname, IN bool isbinary)
{
	if (!pmbs)
		return false;
	else
		return pmbs->SaveAs(fname, isbinary);
}

void MBFileParser::clear()
{
	bodyvec.clear();
	if (!pmbs)
	{
		delete pmbs;
		pmbs = nullptr;
	}
	return;
}

void MBFileParser::CheckId(IN int id)
{
	if (id < 0)
		throw MBException("Body Id error!");
	return;
}

void MBFileParser::CheckMass(IN double m)
{
	if (m <= 0)
		throw MBException("Body Mass error!");
	return;
}

void MBFileParser::CheckJc(IN const Json::Value& Jc)
{
	if (Jc.size() != 6)
		throw MBException("Body Jc dimension error!");
	return;
}

void MBFileParser::CheckRho(IN const Json::Value& rho)
{
	if (rho.size() != 3)
		throw MBException("Joint point vector dimension error!");
	return;
}

void MBFileParser::CheckMat3d(IN const Json::Value& val)
{
	if (val.size() != 9)
		throw MBException("Matrix3d dimension error!");
	return;
}

void MBFileParser::CheckPos(IN const Json::Value& val, IN unsigned int k)
{
	if (val.size() != k)
		throw MBException("Position dimension error!");
	return;
}

void MBFileParser::CheckVel(IN const Json::Value& val, IN unsigned int k)
{
	if (val.size() != k)
		throw MBException("Velocity dimension error!");
	return;
}

void MBFileParser::CheckForce(IN const Json::Value& val, IN unsigned int k)
{
	if (val.size() != k)
		throw MBException("Joint Force dimension error!");
	return;
}

void MBFileParser::GetJc(IN const Json::Value& Jc, OUT Matrix3d& Ic)
{
	for (unsigned int j = 0; j < 3; ++j)
		Ic(0, j) = Jc[j].asDouble();
	for (unsigned int j = 3; j < 5; ++j)
		Ic(1, j - 2) = Jc[j].asDouble();
	Ic(2, 2) = Jc[(unsigned int)5].asDouble();
	for (unsigned int i = 1; i < 3; ++i)
		for (unsigned int j = 0; j < i; ++j)
			Ic(i, j) = Ic(j, i);
	return;
}

void MBFileParser::GetRho(IN const Json::Value& rho, OUT Vector3d& r)
{
	for (unsigned int i = 0; i < 3; ++i)
		r(i) = rho[i].asDouble();
	return;
}

void MBFileParser::GetMat3d(IN const Json::Value& val, OUT Matrix3d& M)
{
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
			M(i, j) = val[i * 3 + j].asDouble();
	}
	return;
}

void MBFileParser::GetPos(IN const Json::Value& val, OUT VectorXd& p)
{
	p.resize(val.size());
	for (unsigned int i = 0; i < val.size(); ++i)
		p(i) = val[i].asDouble();
	return;
}

void MBFileParser::GetVel(IN const Json::Value& val, OUT VectorXd& v)
{
	v.resize(val.size());
	for (int i = 0; i < v.rows(); ++i)
		v(i) = val[i].asDouble();
	return;
}

void MBFileParser::GetForce(IN const Json::Value& val, OUT VectorXd& f)
{
	f.resize(val.size());
	for (unsigned int i = 0; i < f.rows(); ++i)
		f(i) = val[i].asDouble();
	return;
}
